package ru.usu.gv.gui.routing;

import ru.usu.gv.utils.Pair;
import ru.usu.gv.utils.geometry.Polygon;
import ru.usu.gv.utils.geometry.Polyline;

import javax.vecmath.Point2d;
import javax.vecmath.Point3d;

import java.awt.Color;
import java.util.ArrayList;
import java.util.Comparator;
import java.util.HashMap;
import java.util.HashSet;
import java.util.List;
import java.util.Map;
import java.util.Set;
import java.util.SortedSet;
import java.util.TreeSet;

/**
 * @author spupyrev
 * 28.10.2009
 */
public class VisibilityGraph
{
	private Map<Point2d, Set<Point2d>> edges;

	public VisibilityGraph(Point2d source, Point2d target, List<Polygon> obstacles)
	{
		edges = new HashMap();
		edges.put(source, new HashSet());
		edges.put(target, new HashSet());
		List<Polygon> properObstacles = new ArrayList<Polygon>();
		for (Polygon obstacle : obstacles)
			if (!obstacle.contains(source) && !obstacle.contains(target))
				properObstacles.add(obstacle);

		obstacles = properObstacles;
		for (Polygon obstacle : obstacles)
			for (Point2d point : obstacle.getPoints())
				edges.put(point, new HashSet());

		for (Point2d p1 : edges.keySet())
			for (Point2d p2 : edges.keySet())
				if (!intersectAny(p1, p2, obstacles))
					edges.get(p1).add(p2);
	}

	private boolean intersectAny(Point2d p1, Point2d p2, List<Polygon> obstacles)
	{
		for (Polygon obstacle : obstacles)
			if (obstacle.intersect(p1, p2))
				return true;
		return false;
	}

	public Polyline getShortestPolyline(Point2d source, Point2d target)
	{
		Map<Point2d, Double> dist = new HashMap();
		Map<Point2d, Point2d> parent = new HashMap();
		dist.put(source, 0.0);
		SortedSet<Pair<Double, Point2d>> prq = new TreeSet<Pair<Double, Point2d>>(new Comparator<Pair<Double, Point2d>>()
		{
			public int compare(Pair<Double, Point2d> o1, Pair<Double, Point2d> o2)
			{
				int fRes = o1.getFirst().compareTo(o2.getFirst());
				if (fRes != 0)
					return fRes;

				int sRes = new Double(o1.getSecond().x).compareTo(new Double(o2.getSecond().x));
				if (sRes != 0)
					return sRes;

				return new Double(o1.getSecond().y).compareTo(new Double(o2.getSecond().y));
			}
		});
		prq.add(new Pair(0.0, source));

		while (!prq.isEmpty())
		{
			Pair<Double, Point2d> now = prq.first();
			prq.remove(now);

			Point2d p = now.getSecond();
			for (Point2d neig : edges.get(p))
			{
				if (!dist.containsKey(neig) || dist.get(neig) > dist.get(p) + p.distance(neig) + 1e-7)
				{
					Pair<Double, Point2d> pair = new Pair(dist.get(p) + p.distance(neig), neig);
					if (prq.contains(pair))
						prq.remove(pair);

					prq.add(pair);
					dist.put(pair.getSecond(), pair.getFirst());
					parent.put(neig, p);
				}
			}
		}

		Polyline polyline = new Polyline();
		if (!parent.containsKey(target))
			throw new RuntimeException("target is not reachable!");

		Point2d cur = parent.get(target);
		while (cur != source)
		{
			polyline.addPoint(cur);
			cur = parent.get(cur);
		}

		polyline.reverse();
		return polyline;
	}

	public void debugShow()
	{
		for (Point2d p1 : edges.keySet())
			for (Point2d p2 : edges.get(p1))
			{
				DebugUtilities.showLine(Color.RED, new Point3d(p1.x, p1.y, 0), new Point3d(p2.x, p2.y, 0));
			}

		for (Point2d p1 : edges.keySet())
		{
			DebugUtilities.showCircle(Color.RED, new Point3d(p1.x, p1.y, 0), 0.01);
		}
	}
}
